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The  Brown  Water  Navy  doctrine  recently  issued 
by  DoD  calls  for  the  Naval  ships  to  operate  in  a  close 
vicinity  of  the  enemy  shore.  Furthermore,  the  Navy 
foresees  increasing  reliance  on  the  use  of  unmanned 
air  vehicles  (UAVs)  for  reconnaissance  and  other 
missions.  This  combination  of  greater  utilization  of 
UAVs  while  operating  in  proximity  of  the  enemy 
has  highlighted  the  necessity  of  using  stealth  in 
the  recovery  of  UAVs  by  the  Naval  ships.  In  other 
words,  the  ship  will  not  jeopardize  its  security  by 
communicating  its  position  to  the  UAV.  Therefore, 
this  consideration  rules  out  employing  such  position 
sensors  as  GPS  and  places  emphasis  on  other  passive 
sensors.  Clearly,  the  only  passive  sensors  capable 
of  providing  relative  position  information  are  vision 
based.  Moreover,  since  UAV s  are  expected  to  operate 
around  the  clock  and  in  all  weather  conditions 
infrared  (IR)  cameras  are  the  passive  sensors  of 
choice. 

The  UAV  shipboard  autoland  includes  finding 
the  ship,  constructing  a  landing  trajectory  based 
on  the  relative  position,  velocity  and  orientation 
information  (the  navigation  solution)  obtained  from 
passive  sensors,  and  then  tracking  this  trajectory 
using  onboard  control  system.  We  are  not  considering 
the  landing  itself  because  at  short  ranges  low-power 
communication  between  UAV  and  the  ship  is  allowed. 

Determining  the  navigation  solution  with  respect 
to  the  ship  using  passive  sensors  can  be  divided  into 
two  distinct  phases:  1)  at  large  distances  the  ship  is 
seen  as  a  single  hot  spot  by  the  onboard  IR  camera; 

2)  at  closer  distances  additional  features  can  be 
determined  (see  Fig.  1).  Phase  1  has  been  addressed 
in  our  previous  work,  see  [1,  2],  where  new  filtering 
algorithms  have  been  developed  that  integrate  IR  and 
inertial  navigation  system  (INS)  sensor  systems  to 
obtain  relative  position  and  velocity  of  a  UAV  with 
respect  to  a  ship.  These  algorithms  are  designed  to 
handle  out-of-frame  events  and  occlusions  that  are 
common  to  vision  sensors.  In  this  work  we  obtain 
the  navigation  solution  for  the  Phase  2.  Specifically, 
the  problem  of  determining  range  and  orientation 
of  an  aircraft  to  a  ship,  which  has  a  minimum  of 
three  identifiable  points  is  addressed.  The  filtering 
algorithms  developed  in  [1,  2]  that  use  a  single  point 
have  been  used  to  initialize  the  algorithm  developed 
here.  Once  again  this  algorithm  is  supposed  to 
help  bring  the  UAV  as  close  to  the  ship  as  possible 
(while  all  three  reference  points  are  visible  in  the  IR 
camera).  After  this  any  final  landing  procedure  may 
be  implemented. 

The  visual  range  at  which  a  ship  on  the  surface  of 
the  sea  can  be  located  depends  on  the  contrast  of  the 
ship  to  its  surroundings  according  to  Koshmieder’s 
relation  [3,  4],  The  visual  range  centered  around 
0.55  micrometers  depends  on  weather  conditions  and 
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Fig.  1.  IR  images  of  ship  at  decreasing  distances  from  the  camera,  (a)  Over  5  mi.  (b)  Between  3  and  5  mi.  (c)  Below  3  mi. 


ambient  light,  which  can  produce  glare  obscuring  the 
ship.  Although  one  can  often  detect  ships  at  great 
distances  in  daylight,  the  visible  spectrum  is  severely 
limited  under  poor  weather  conditions  and  especially 
at  night  for  military  vessels  without  running  lights 


[5].  Since  ah  powered  ships  will  radiate  strongly  in 
the  8-12  micrometer  IR  atmospheric  window,  it  is 
preferable  to  locate  ships  by  their  hot  smokestack  and 
engine.  Use  of  the  IR  greatly  simplihes  the  problem 
of  locating  a  ship  and  reduces  susceptibility  to  glare. 
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Fig.  2.  IR  images  of  naval  ship. 


(a)  (b) 

Fig.  3.  Extraction  of  ship  from  background. 


The  visible  spectrum  may  be  used  to  supplement 
the  IR  for  long-range  detection  in  clear  daylight 
conditions. 

The  hot  IR  smokestack  reliably  gives  the 
location  of  the  ship  with  minimal  image  processing 
as  compared  with  visible  light.  Examples  of  the 
information  available  at  different  ranges  are  presented 
in  Fig.  1  in  a  contour  plot  and  surface  plot  overlaying 
the  image.  At  the  limit  of  the  detection  range  for  a 
given  minimum  resolvable  temperature  difference, 
the  only  detectable  point  may  be  the  smokestack 
(Fig.  1(a)),  but  at  closer  range  there  will  be  many 
relatively  hot  points  that  can  be  recovered  using  image 
processing  (Fig.  1(b), (c)). 

However,  only  a  few  of  the  hottest  points  projected 
onto  the  focal  plane  of  the  IR  camera  can  be  identified 
reliably  at  great  distances.  This  observation  naturally 
leads  to  the  following  critical  question:  what  is 
the  minimum  number  of  known  reference  points 
(RPs)  that  is  necessary  to  determine  the  range  and 
orientation  of  the  IR  camera  with  respect  to  the 
ship.  It  turns  out  the  answer  to  this  question  is  three. 
But  using  only  three  points  always  results  in  more 
than  one  solution  as  has  been  shown  by  a  number 
of  researchers  in  the  areas  ranging  from  projective 
geometry  to  photogrammetry.  Indeed,  a  survey  of  the 
scientific  literature  reveals  that  the  number  of  possible 
solutions  may  range  from  four  to  fifteen.  (A  detailed 
discussion  of  these  results  is  given  in  Section  III.) 

This  problem  of  nonuniqueness  is  usually  resolved 
at  very  close  ranges  by  using  more  than  three  points 


that  must  lie  in  the  same  plane  (see  for  example  [6-8] 
and  references  therein).  However,  at  greater  ranges 
the  computational  cost  of  obtaining  each  additional 
known  point  that  lies  in  plane  defined  by  the  initial 
three  becomes  prohibitive  for  real-time  applications. 
Therefore,  we  assume  here  that  three  reliable  points 
may  be  computed  from  the  location  of  the  smokestack 
and  the  extents  (width  and  height)  of  the  ship.  An 
illustration  of  how  these  images  of  three  RPs  can  be 
obtained  from  an  IR  image  of  the  ship  is  given  next. 

Fig.  2  presents  examples  of  IR  images  of  the 
naval  ship  passing  through  San  Diego  harbor  at 
two  different  distances  of  more  than  two  miles.  The 
images  are  shown  in  contour  and  surface  plot  above 
a  false  color  image  to  illustrate  the  information  that 
is  available.  Using  previously  developed  algorithms 
[5]  the  ship  may  be  extracted  from  the  background 
as  shown  in  Fig.  3.  The  false-color  image  of  the  ship 
has  been  automatically  located  and  indicated  by  a  box 
in  the  first  insert.  The  next  two  inserts  are  the  single 
level  binary  and  gray-scale  images  of  the  ship  that 
have  been  extracted  from  the  background. 

Finally,  Fig.  4  shows  images  of  three  RPs  that 
can  be  used  by  the  algorithm  developed  here.  These 
images  may  be  obtained  as  follows:  the  smokestack 
by  using  thresholding  of  the  image  directly,  while 
the  other  two  points  by  intersecting  the  images  of  the 
edges  of  the  ship’s  deck. 

Having  shown  how  the  ship  may  be  located  and 
the  three  points  of  information  (images  of  RPs) 
established,  we  focus  our  attention  on  determining 
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(a) 


(b) 


Fig.  4.  Examples  showing  images  of  3  RPs. 


Fig.  5.  Three-point  geometry  applied  to  shipboard  navigation. 


the  range  and  orientation  of  an  IR  camera  with  respect 
to  the  ship  using  images  of  three  RPs  (see  Fig.  5).  To 
address  the  problem  of  non-uniqueness  of  the  solution 
we  introduce  a  concept  of  an  admissible  solution  and 
using  extensive  numerical  analysis  show  that  under 
reasonable  assumptions  on  the  relative  geometry  of 
the  camera  and  RPs  there  can  be  at  most  two  such 
solutions.  (The  admissible  solution  implies  that  the 
camera  is  in  front  of  the  ship.)  Based  on  this  analysis 
we  develop  an  efficient  numerical  algorithm  that 
identifies  the  two  admissible  solutions  and  selects  the 
correct  one.  The  utility  of  the  algorithm  is  illustrated 
in  simulation  and  using  flight  test  data  collected  by  an 
IR  camera  mounted  on  a  small  UAV. 

This  paper  is  organized  as  follows.  Section  II 
contains  a  mathematical  formulation  of  the  problem. 
Section  III  describes  previous  work  in  this  area,  which 
dates  back  to  1841.  Section  IV  contains  numerical 
analysis  of  the  problem  and  discusses  proposed 
numerical  solution.  Results  of  computer  simulation  are 
shown  in  Section  V.  Sections  VI  and  VII  discuss  the 


flight  test  setup  and  present  the  results  obtained  using 
flight  test  data.  The  paper  ends  with  conclusions. 

II.  PROBLEM  FORMULATION 

Consider  Fig.  6.  Let  p-  =  {Xj,yj,Zj},  i  =  1,...,3 
denote  the  vectors  connecting  the  origin  of  the  camera 
frame  O  with  the  three  known  points  P-,  i  =  1,...,3. 
Let  di,  i  =  1,...,3  denote  distances  between  these 
points: 

IIP1-P2II  =^l  7^0’  IIP1-P3II  =^2  7^0’ 
l|P2  “  P3  II  ~  ^3  7^  7^  ^2  7^  ^3 

and  =  ||p,||,  i  =  1,...,3  denote  the  norms  of  the 
vectors  Pj.  We  utilize  the  pinhole  camera  model  [9]. 
Using  this  model  the  projection  of  each  RP  onto  the 
image  plane  of  the  camera  with  the  focal  length  /  has 
the  following  form: 
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Fig.  6.  3P3  geometry. 


di 


Now  by  combining  (1)  and  (2)  we  obtain  nine 
equations  in  nine  unknowns  {x^,y^,Zi},  i  = 

Using  (2)  we  get 


yt  = 


f  ’ 


Zi  = 


f 


(3) 


By  substituting  these  expressions  into  (1)  and  after 
simple  algebra  we  can  reduce  (2)  and  (1)  to  a  set  of 
three  nonlinear  equations  in  three  unknowns: 


+  wf  +  vf)xf  -  2{f  +  MjMj  +  v^v^)x^x^  =  {fd^f 

;  =  1,2 


+  wf  +  v^)x]  -  2{f  +  MjMj  +  VJV3).r^X3  =  {fd^f  (-4^ 

1  =  1,3 


+  wf  +  v'^)x^  -  2{f  +  M2M3  +  V2V3)X2X3  =  {fd^f. 

1=2,3 


To  simplify  notation  we  rewrite  (4)  as  follows 
Ax\  —  2Dj23CjX2  +  Bx\  =  di 
Ax\  —  2Dj3X^X3  +  Cxi  -  ^2 

BX'2  ^^^23'^2'^3  ^“^3  —  ^3' 

Note,  that  the  coefficients  A,  B,  C ,  d^,  i  =  .  .,3  are 

strictly  positive  by  construction. 

Using  (5)  one  can  obtain  another  system  of 
equations  better  suited  for  further  analysis.  First, 
observe  that 


^  VC 


Now  by  rewriting  system  (5)  in  terms  of  s^,  i  =  1, 
we  get 

sj  —  251^2  cos  +  si  =  d\ 

sj  —  25153  cos  a2  +  si  =  dl 

S'2  2S'2^S'^  cos  013  “1“  53  —  r/3 


where 

cosaj 

cosa3 


\P\\\\\P2 

ip2’p3) 

\P2\\\\P3 


cosa2  = 


(P0P3) 

\Pl\\\\p3 


(6) 

.,3 

(7) 


(see  Fig.  6). 


Obviously  system  (7)  has  an  upper  bound  of 
eight  (2  X  2  X  2)  real  solutions.  Moreover  they  form 
four  symmetric  pairs,  because  if  a  triplet  (5j,52,5p 
is  a  solution,  than  the  triplet  (— 5^—52,— 5^  forms  a 
solution  as  well. 

Geometrically  system  (7)  can  be  described  as  an 
intersection  of  three  orthogonal  elliptic  cylinders  with 
the  semiaxes  rotated  around  corresponding  symmetry 
axes  by  the  angle  of  45°.  This  follows  directly  from 
the  canonical  form  of  equation  (7).  The  magnitudes  of 
the  semiaxes  for  each  cylinder  are  equal  to 


= 


d^ 


^/T± 


cos  a; 


i  =  1,...,3. 


(8) 


It  is  clear  that  the  intersection  of  any  two 
cylinders  is  always  nonempty  and  the  number  of 
solutions  in  this  case  is  infinite.  However,  by  adding 
a  third  cylinder  one  can  get  only  a  finite  number 
of  intersection  points.  In  practice  for  the  system  (7) 
this  number  cannot  be  zero  or  two  (as  will  be  shown 
in  Section  IV).  The  only  possible  set  of  solutions 
contains  four,  six,  or  eight  points.  For  instance. 

Fig.  7(a)  demonstrates  an  example  with  four  real 
solutions  to  system  (7)  (two  pairs  of  symmetric 
points).  Increasing  the  size  of  the  cylinder  along  the 
52  axis,  results  in  three  pairs  of  solutions  (Fig.  7(b)). 
Further  increase  leads  to  four  pairs  (Fig.  7(c))  and 
again  to  three  pairs  of  solutions  (Fig.  7(d)).  Eventually 
only  two  pairs  of  solutions  remain  (Fig.  7(e)). 

In  the  work  reported  here,  we  make  the  following 
assumption. 

Al.  The  camera  is  always  in  front  of  the  plane 
defined  by  three  RPs  P-,  i  =  1,...,3. 

In  the  sequel  the  set  of  all  vectors 
i  =  1,...,3  that  satisfy  Assumption  Al  is  called 
admissible.  Assumption  Al  implies  that  the 
x-component  of  each  vector  in  the  admissible  set  is 
positive  (i.e.,  Si  >0,i  =  1,...,3). 

Summarizing,  the  problem  to  be  addressed 
here  is  to  find  all  admissible  solutions  to  (7)  using 
Assumption  Al  and  two  additional  assumptions 
discussed  in  Section  IV.  Furthermore,  since  the  set  of 
admissible  solutions  contains  more  than  one  element 
we  would  like  to  develop  a  test  to  select  the  correct 
solution. 


III.  PREVIOUS  WORK 

It  turns  out  that  the  three-point  perspective  pose 
estimation  problem  (P3P)  (as  the  problem  addressed 
here  is  called  in  computer  vision)  was  first  formulated 
by  the  German  mathematician  Grunert  in  1841 
([10]).  Since  then  it  has  been  addressed  by  many 
scientists  throughout  the  world.  As  a  result  it  has  been 
well  established  that  the  problem  does  not  have  an 
analytical  solution  and  most  attempts  were  directed 
at  getting  a  numerical  one.  In  the  remainder  of  this 
section  we  present  a  brief  overview  of  the  existing 
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(C) 


m 


Fig.  7.  Examples  of  possible  geometry  for  system  (7).  (a)  and  (e)  Two  solutions,  (b)  and  (d)  Three  solutions,  (c)  Four  solutions. 


results  partly  based  on  reference  by  Haralick,  Lee, 
Ottenberg,  and  Nolle  ([11],  1991). 

According  to  Muller  ([12],  1925)  Grunert  obtained 
(7)  by  simple  use  of  the  law  of  cosines  implemented 
for  the  corresponding  tetrahedron.  Most  of  the  work 


addressing  this  problem  used  formulation  (7)  as  well. 
Grunert  himself  introduced  two  new  variables 


u*  =  —  and  V*  =  —  (9) 

N  *^1 
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to  show  that  with  their  help  system  (7)  can  be  reduced 
to  a  fourth-order  polynomial  with  respect  to  v*.  Since 
the  coefficients  of  this  polynomial  were  expressed 
as  complicated  functions  of  the  problem  data,  this 
polynomial  could  be  neither  solved/simplified 
analytically  nor  analyzed.  This  is  due  to  the  fact  that, 
in  general,  the  roots  of  the  fourth  order  polynomial 
cannot  be  obtained  analytically  (see  for  example  [13] 
and  references  therein). 

The  same  approach  was  used  by  Merritt  ([14,  15], 
1949)  and  independently  by  Fischler  and  Bolles 
([6],  1981).  With  the  same  substitution  and  by 
manipulating  different  pairs  of  equations  and  different 
multipliers,  they  reduced  the  problem  to  a  fourth-order 
polynomial  in  terms  of  u*,  rather  than  v*. 

Several  attempts  to  decrease  the  order  of  the  final 
polynomial  to  be  solved  are  known  as  well  ([11]).  For 
example,  Finsterwalder  ([16],  1903)  instead  of  finding 
all  roots  of  a  fourth-order  polynomial  reduced  the 
problem  to  finding  a  root  of  a  cubic  polynomial  and 
the  roots  of  two  quadratic  polynomials.  Grafarend, 
Lohse,  and  Schaffrin  ([17],  1989)  applied  different 
transformations  to  (7)  in  order  to  reduce  the  problem 
to  finding  the  same  roots  of  a  cubic  polynomial 
and  the  roots  of  two  quadratic  polynomials.  In  [18] 
Lohse  further  extended  these  results  to  show  that 
admissible  solutions  can  be  picked  up  from  as  many 
as  15  solutions  provided  by  his  transformations. 

Linnainmaa,  Hanvood,  and  Davis  ([19],  1988), 
using  another  and  more  effective  substitution,  that  is 


Fig.  8.  Solutions  shown  in  Fischler  and  Bolles  [6].  (a)  Singular 
case,  (b)  General  case. 


Note  that  for  case  of  equilateral  triangle,  system 
(7)  is  reduced  to  the  following 

sj  —  25^52  cos  a  +  s 2  = 

s\  —  2siS2,co'&a  +  si  =  (11) 

si  —  252^3  cos  a  *^3  —  d^- 

By  subtracting  the  second  equation  from  the  first,  we 
obtain 


(*^2  “ +  *^3  “  2*^1  cosa)  =  0.  (12) 

Equating  S2  and  53  in  the  third  equation  of  system  (11) 
we  get 


d 

V2(l  -cosa)" 


(13) 


S2  =  u*  +  s^cosai  and  53  =  v* -1- cosa2 

(10) 

reduced  system  (7)  to  fourth-order  polynomial  in  sj. 

Quan  and  Lan  ([8],  1999)  mentioned  that  they 
could  use  classical  Sylvester  resultant  to  rewrite  (7) 
as  an  eighth-order  polynomial  in  (fourth-order 
polynomial  in  j'j). 

Today  availability  of  powerful  computers  has  made 
it  easy  to  find  all  possible  solutions  to  either  three-  or 
four  or  eight-order  polynomial.  Moreover,  Haralick, 
Lee,  Ottenberg,  and  Nolle  [11]  did  compare  numerical 
accuracy  of  possible  solutions  obtained  using  all 
approaches  mentioned  in  this  section  (and  showed 
by  the  way  that  only  approaches  by  Fischler  and 
Bolles,  and  Linnainmaa,  Hanvood,  and  Davis  involve 
no  singularity  in  the  computation).  But  the  questions 
of  what  is  the  number  of  admissible  solutions  for 
the  specific  problem  geometry  and  how  to  select  the 
correct  one  still  have  not  been  completely  answered. 

To  show  that  at  most  four  admissible  solutions 
can  be  found,  Fischler  and  Bolles  considered  a 
specific  case  of  equilateral  triangle  P1P2P3  (see  Fig.  6) 
where  d^  =  2s/3,  cosa^-  =  5/8,  i  =  1,...,3,  i.e.,  when 
system  (7)  becomes  singular.  For  this  case  they 
obtained  numerically  four  admissible  solutions  shown 
graphically  in  Fig.  8(a). 


Finally,  either  the  first  or  the  second  equation  provides 
two  solutions  for  : 


_  f  d(2cosa—  1)  I 
■  PV2(l-cos<^J  • 


(14) 


Note  that  due  to  symmetry,  four  admissible 
solutions  can  be  obtained  for  this  particular  case 
(the  second  multiplier  in  (12)  gives  the  same 
nonsymmetric  roots).  Moreover,  four  admissible 
solutions  exist  for  a  more  general  case,  when  only  two 
of  the  three  equations  in  (7)  are  singular,  i.e.,  when 
triangle  P1P2P2,  is  isosceles  and  camera  resides  in  the 
symmetry  plane  (these  solutions  can  be  obtained  in 
the  same  manner  as  was  done  in  (11)-(14)).  Now,  the 
natural  question  to  ask  is  could  it  still  be  the  general 
case? 

To  answer  this  question  Fischler  and  Bolles 
propose  the  following  procedure  to  obtain  four 
admissible  solutions  (see  Fig.  8(b)).  Moving  along  the 
line  OP^  (see  Fig.  8(b))  and  fixing  the  pair  of  points 
{'^3’'^3}  corresponding  to  edges  d^  and 
^2,  respectively,  one  obtains  four  candidate  solutions 
{{P2A^,  {Pi-,Pi\,  {Pl'A^,  and  [P2;^3'])-  In  the  general 
case,  these  candidate  solutions  have  different  lengths 
that  do  not  equal  d^.  Therefore,  Fischler  and  Bolles 
suppose  that  each  of  them  can  be  equal  to  d^  for  a 
certain  resulting  in  four  admissible  solutions.  That 
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e)z=0.2 


Fig.  9.  PASS  for  example  of  Fischler  and  Bolles  [6]. 


is  true.  However,  this  is  true  only  for  singular  cases 
discussed  above  and  for  a  certain  configuration  of 
problem  data  in  the  general  case  (as  shown  in  the  next 
section). 

The  PSP  is  also  well  known  in  the  field  of 
photogrammetry.  The  term  photogrammetry  came 
into  general  use  in  the  U.S.  about  1934,  although 
the  term  already  had  been  widely  used  in  Europe 
since  1893  after  German  A.  Meydenbauer  [20,  21]. 
The  main  objective  of  photogrammetry  is  to  obtain 
reliable  landscape  measurements  by  means  of  aerial 
photographs.  Since  tilted  photographs  introduce 
errors  in  the  map  position,  it  is  important  to  take  into 
account  tilt  and  swing  in  aerial  photographs  at  the 
time  of  exposure.  This  task  is  one  of  the  fundamental 
problems  in  the  photogrammetry  and  is  called  “space 
resection  involving  the  determination  of  the  spatial 
position  of  a  camera  exposure  station.”  Despite 
numerous  attempts  to  solve  system  (7)  analytically,  the 
only  solution  that  is  in  use  in  photogrammetry  today 


is  a  numerical  solution  developed  by  E.  Church  in  the 
mid  1930s  [22,  23,  9]. 

Church’s  approach  considers  two  pyramids — 
ground  pyramid  with  three  RPs  and  exposure  center 
of  a  camera,  and  image-plane  pyramid  with  three 
image  points  and  the  same  center  (see  Eig.  6). 

This  procedure  finds  a  solution  that  makes  the  two 
pyramids  coincide  and  can,  in  fact,  be  interpreted  as 
a  well-known  method  of  Newton’s  iterations.  This 
procedure  seems  to  work  well  when  the  initial  guess 
is  sufficiently  close  to  the  correct  admissible  solution. 
However,  Church’s  method  does  not  address  the  issue 
of  the  nonuniqueness  of  the  solution.  It  improves  on 
an  initial  guess,  which  has  to  be  quite  accurate  (within 
a  few  percentage  points  of  the  true  solution  [24,  20]). 
Otherwise  Church’s  method  does  not  guarantee 
convergence  to  the  correct  solution.  Moreover,  even 
with  a  good  initial  guess  this  method  converges  only 
if  the  exposure  station  is  “high  over  the  ground  and 
located  inside  the  cylinder  or  the  sphere  containing 
three  RPs”  [20]. 
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(a)  z  =  5.0 


(b)  z  =  2.0 


(c)  z  =  0.6 


(d)  z  =  0.2 


Fig.  10.  Cross-sections  of  PASS  for  general  geometry  of  RPs. 


IV.  NUMERICAL  ANALYSIS 

In  this  section  we  present  results  of  the  numerical 
analysis  of  the  PSP.  The  critical  issue  addressed 
includes  determining  geometry  of  the  feasibility 
regions  (i.e.,  the  regions  that  have  two,  three,  or  four 
admissible  solutions).  First  these  regions  are  computed 
for  the  example  used  by  Fischler  and  Bolles  [6],  who 
considered  the  case  where  RPs  form  an  equilateral 
triangle.  Then  a  similar  exercise  is  performed  for  the 
case  of  an  arbitrary  triangle.  This  exercise  suggests 
that  the  shape  of  the  feasibility  regions  is  complex  and 
invariant  of  the  shape  of  the  triangle  formed  by  the 
RPs.  Finally,  an  algebraic  analysis  motivated  by  the 
geometric  solution  proposed  by  Fischler  and  Bolles 
is  given.  This  analysis  is  used  to  develop  an  efficient 
numerical  algorithm  to  solve  the  PSP. 

Fig.  9  contains  the  results  of  a  numerical  analysis 
of  the  specific  example  of  an  equilateral  triangle  given 
in  Fischler  and  Bolles  [6].  Here  the  shaded  areas 
represent  the  set  of  points  where  four  admissible 
solutions  (FASS)  to  the  PSP  exist.  In  particular. 


left-bottom  part  of  Fig.  9  shows  the  SD  view  of  this 
solution  set  parameterized  by  the  elevation  above  the 
plane  formed  by  the  triangle.  Other  plots  in  the  figure 
show  the  top  view  of  each  level  set.  Fig.  9  clearly 
shows  that  FASS  can  be  obtained  not  only  at  the 
point  of  complete  symmetry  (central  point  on  Fig.  9 
at  z  =  0.2, ...,5.0)  as  shown  in  [6],  but  at  many  other 
points  as  well.  In  fact,  one  can  see  that  the  geometry 
of  FASS  is  fairly  complex.  This  explains  why  it  has 
been  so  difficult  to  characterize  FASS  analytically  and 
why  Church’ s  iterative  procedure  does  not  converge 
on  its  boundary  (see  Section  III). 

In  Fig.  10  FASS  is  computed  for  the  case  where 
RPs  form  an  arbitrary  triangle.  It  suggests  that  the 
shape  of  FASS  is  invariant  of  the  shape  of  the  triangle 
formed  by  RPs.  Together,  Figs.  9  and  10  suggest  that 
FASS  is  a  complex  inverted  “pyramid”  that  is  normal 
to  the  plane  formed  by  the  three  RPs.  At  its  base  it 
forms  a  circle  that  contains  the  triangle  generated  by 
the  RPs. 

Now  we  make  the  following  additional 
assumptions. 
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Fig.  11.  Illustration  of  two  sets  of  solutions. 


Fig.  12.  Illustration  of  two  admissible  solutions  for 


A2.  min.^i  3^.  >  max-^j  3^/.. 

A3.  The  camera  resides  outside  of  PASS. 
Assumption  A2  implies  that  the  camera  is  sufficiently 
far  from  the  ship.  As  is  shown  next.  Assumptions 
A1-A3  guarantee  that  the  P3P  addressed  in  this  work 
has  only  two  admissible  solutions. 

First  observe  that  inside  the  region  that  satisfies 
A1-A3  the  following  inequalities  hold 

3 

0<a,.  <7r/2,  <7r,  ^  a^, 

'  =  1  /=TfT3;/^i 

i  =  l,...,3.  (15) 


Now  from  first  two  equations  in  (7)  we  obtain  the 
following  expressions  for  S2  and  53: 

=  cos ±  ^ (cosa,_i -  (s^  - 

i  =  2,3.  (16) 


From  (16)  it  is  clear  that  the  set  of  all  possible 
admissible  solutions  for  lies  in  the  following 
interval 


0  <  j'l  <  =  min 

i  =  l,2 


A~= 


COS^  O; 


=  min 

1=1,2 


sin  O;  J 


(17) 

Furthermore,  since  due  to  (15)  sina^  >  0,  i  =  1,2,  this 
interval  is  never  empty.  We  first  consider  the  case 


d  1  .  d^ 

- -  7^  ^ - • 

smoj  sma2 

By  substituting  the  expressions  for  S2  and  53  given 
by  (16)  into  the  last  equation  of  (7)  we  obtain 


four  equations  in  Let 


A, 


COSO;  5  j  + 


2 


—  2  COS  03  jQ  (  cos  +  sj dj  —  s\  sin^  \  —d^. 

;  =  1,2 

(18) 

Similarly  define  ^ _ ’  obtained  by 

taking  all  possible  combinations  of  expressions  (16). 
Notice,  by  setting  each  of  these  expressions  to  zero: 


A^^(5i)  =  0  A_^(5i)  =  0 

A^_(5i)  =  0  A__(5i)  =  0 


(19) 


we  obtain  admissible  solutions  for 

Consider  Fig.  11,  which  includes  the  plots  of 

A3.3.,  A+_,  and  A _ versus  It  can  be  seen  that 

solving  (19)  for  will  result  in  two  sets  of  solutions, 
one  of  which  is  admissible. 

In  Fig.  12  the  area  that  contains  two  admissible 
solutions  in  Fig.  1 1  is  magnified.  Clearly,  the  set  of 
admissible  solutions  for  may  contain  one  or  two 
elements.  One-element  case  results  from  the  fact 
that  either  S2  or  53  in  (16)  have  one  solution,  which 
leads  us  to  the  conclusion  that  On  the  other 

hand,  in  the  two-element  case  both  S2  and  have  two 
solutions.  Finally  observe  that  due  to  Assumption  A2 
none  of  the  following  expressions  can  be  zero  when 
evaluated  at  =  0: 

^++(0)  =  A _ (0)  =  d\  —  2d^d2COsa2  ^-d^  —  d^ 

=  2d^dAcosP,P.P^  —  cos a,)  <  0, 

(20) 

A3._(0)  =  A_3.(0)  =  d\  +  26?j6?2Cosa3  +  d^  —  d^ 

=  2fi?j6?2(cosP3PiP2  +  cos 03)  >  0. 


1190  IEEE  TRANSACTIONS  ON  AEROSPACE  AND  ELECTRONIC  SYSTEMS  VOL.  38,  NO.  4  OCTOBER  2002 


Authorized  licensed  use  limited  to:  Naval  Postgraduate  School.  Downloaded  on  March  1 1 ,2010  at  15:34:47  EST  from  IEEE  Xplore.  Restrictions  apply. 


Thus,  the  functional  dependence  of  the  expressions 
^+  +  (si),  A_+(5i),  A+_(5i),  A__(5i)  on  will 
always  have  the  form  shown  in  Figs.  11  and  12,  i.e., 
when  Assumptions  A1-A3  hold  only  two  admissible 
solutions  can  only  be  obtained. 

Next  we  consider  the  case 

d  j  ^^2 

sinaj  sina2' 

Using  previous  arguments  we  conclude  that 


sma^  sma2 

This  leads  to 

Si  =  cosa,._i5i,  di_i  =  smai_iSi, 

i  =  2,3.  (21) 

Substituting  these  expressions  into  the  last  equation  of 
(7)  we  obtain 

dn 

=  ! 

V  Ei=i,2COs2a,.  -  2n?=i  cos  a,. 

and 

d^  _  <^2 

y^l  —  cos^aj  y^l  —  cos2a2 

_ _ ^ _ 

\J E,= 1,2  cos2  a .  -  2  n?.  1  cos  a. 

(22) 

which  results  in  a  unique  solution  for  all  5,-,  i  = 

1,...,3.  Is  this  geometrically  possible?  Notice  that 
the  expression  (21)  shows  that  the  angles  between 
edges  S2  and  d^,  and  .^3  and  ^2  ^c  90°  (see  Fig.  6). 

But  since  the  solution  is  unique,  by  expressing  j'j  in 
terms  of  S2  and  ^3,  due  to  symmetry  we  obtain  that 
j-j  =  cosa;_jj';,  di_i  =  sina;_^.s';,  i  =  2,3.  This  leads  us 
to  the  conclusion  that  the  angles  between  the  edges 
and  d^  and  and  ^2  ^c  90°  as  well,  i.e.,  the  triangle 
contains  two  right  angles.  Therefore,  the  case  with  one 
admissible  solution  is  not  realizable  (it  means  that  the 
camera  is  located  at  infinity  with  respect  to  the  plane 
generated  by  the  three  points).  This  statement  can 
also  be  proved  algebraically.  Replace  in  the  right 
hand  side  of  the  denominator  in  (22)  with  +  012. 
Because  of  (15),  aj  +  a2  >  Using  this  substitution 
the  denominator  can  be  reduced  to  sin  |aj  —  a2|.  Thus, 
the  following  set  of  equations  should  hold 

sina,  =  — ,  sina2  =  — ,  sinlai  —  a;2|  >  — . 

(23) 


the  region  outside  of  FASS  that  satisfies  Assumptions 
A1-A3  contains  two  admissible  solutions. 

Now  for  completeness  we  analyze  what  happens 
to  the  solution  of  the  P3P  as  the  camera  traverses 
FASS.  Consider  Figs.  13(a)-(b).  The  plots  shown  here 
were  obtained  for  the  points  A  through  F  defined  in 
Fig.  9  in  the  same  way  as  the  plots  shown  in  Fig.  12 
(recall  the  intersection  of  each  graph  with  the  x-axis 
determines  an  admissible  solution). 

Notice  the  points  A  and  F  lie  outside  of  FASS  and 
result  in  only  two  admissible  solutions;  point  B  lies  on 
the  boundary  of  FASS  and  produces  three  admissible 
solutions  (it  corresponds  to  the  cases  shown  in  Figs. 
7(b)  and  7(d)).  The  rest  of  the  points  lie  inside  of 
FASS  and  result  in  four  admissible  solutions. 

Since  we  have  shown  that  under  Assumptions 
A1-A3  the  nonsingular  system  (7)  always  has  two 
admissible  solutions,  this  result  can  be  used  to  develop 
a  test  that  selects  the  correct  solution.  The  idea  is  to 
numerically  obtain  both  admissible  solutions  to  (7). 
Then  using  (6)  and  (3)  construct  two  sets  of  vectors 
Pi  A  Pi_2’  *  “  1,...,3.  Then,  compute  the  normals  to 
the  plane  generated  by  each  solution  and  utilize  them 
to  identify  the  correct  one.  Now,  is  it  possible  that  two 
admissible  solutions  have  the  same  normal?  It  is  fairly 
easy  to  see  that  both  solutions  have  the  same  normal 
if  they  are  colinear,  i.e.,  j  =  yupj_2,  i  =  1,...,3,  since 
in  this  case  solutions  must  lie  on  parallel  planes. 

Now  by  applying  condition  (1)  we  deduce  that  11=  1. 
Thus  two  admissible  solutions  always  have  different 
(noncolinear)  normals.  Therefore,  the  correct  solution 
can  be  determined  by  analyzing  normals  generated  by 
each  solution.  Using  normals  to  resolve  ambiguity  is  a 
standard  device  employed  in  the  structure  from  motion 
literature  (see  for  example  [25]).  This  test  will  fail  to 
identify  the  correct  solution  for  the  case  where  camera 
resides  inside  or  on  the  boundary  of  FASS.  This  can 
be  clearly  seen  for  points  B  through  E  in  Fig.  13, 
where  two  or  more  of  the  solutions  are  very  close. 

This  implies  that  resulting  normals  will  be  almost 
colinear.  This  observation  underscores  the  importance 
of  Assumption  A3  for  the  algorithm  presented  next. 

Based  on  the  results  presented  above  we  propose 
the  following  algorithm  for  solving  the  P3R  Suppose 
a  good  initial  guess  of  the  normal  to  the  plane 
generated  by  the  three  points  is  available.  Then,  for 
step  k: 

1)  solve  numerically  (10)  for  ^  in  the  interval 
(17),  using  as  an  initial  guess, 

2)  substitute  each  solution  xf^  obtained  in  1)  into 

-(k)  -(k) 

(3)  to  get  Pi_^  and  p,_2, 

3)  compute  normals 


However,  for  small  values  of  angles  and  a2  this 
leads  to  the  inequality  \di  d2\  >  d^,  which  implies 
that  in  this  case  cosP^Pi  P2>1  (see  Fig.  6).  Therefore, 


2,(k)  ^(k)  Ak)  U*)  ^ 

Ak)  ^  (Pl_l-P2_l)x(Pl_l-P3_l) 
^(k)  ^(k)  ^(k)  -(fe) 

\\Pi_i-P2a\\\\Pia  -P3a\\ 


and 
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Fig.  13.  Illustration  of  nonlinear  behavior  of  solutions  to  (19)  in  the  vicinity  and  inside  PASS. 


-(fc)  ^(k)  ^(k)  ^(k) 

fAk)  ^  (Pl^-  PlJl)  X  (PlJl-  P3^) 

2  ^(k)  ^ik)  -(k)  -  (^)  ’ 

\\Pl_2  ~  PlJlW \\Pl_2  ~  P3J2\\ 

-(k)  -(k) 

4)  choose  set  Pj_i,  i  =  or  i  = 

that  maximizes  the  dot  product  . 

Using  the  solution  provided  by  the  P3P  algorithm 
the  relative  orientation  of  the  aircraft  with  respect  to 
the  plane  formed  by  the  three  RPs  can  be  computed 
as  follows  [26].  Let  {ip}  denote  an  orthogonal 
coordinate  system  attached  to  the  plane  generated 


three  orthogonal  vectors  f],  ^2,  Fg  using  the  correct 
solution  Pi,  p 2,  Pg  as  follows: 

^  _  iP2-Pl)  -  _  (P2-Pl)x(P3-Pl) 

l|P2-Plll  l|P2- Pi  libs -Pill 

^2  =  Pg  X  Pi  .  (24) 

Then  =  [r^  Pg  Pg], 

The  transformation  matrix  g^P  can  also  be 
expressed  using  Euler  angles 


■Uspsmygpsmc^gp 
LcosV^gpSin0gpCos0gp 


+  sm^3p! 


by  the  three  RPs,  let  {c}  denote  the  coordinate  where  V^g^,  O^p,  4>3p  yaw,  pitch,  and  bank  angles, 

system  attached  to  the  camera  and  let  be  the  respectively,  with  respect  to  the  plane  formed  by  the 

coordinate  transformation  from  {3p}  to  {c}.  Form  three  RPs.  Therefore,  one  can  easily  find  the  Euler 
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Fig.  14.  Horizontal  projection  of  aircraft’s  and  ship’s  trajectories. 


Fig.  15.  3D  representation  of  simulation  scenario. 


angles  in  the  following  manner: 

Tit 

=  arctan— ,  ^3^  =  —  arcsinrjg, 

^11 

03p  =  arctan— .  (26) 

^33 

In  general  case  the  coordinate  system  {ip}  does 
not  coincide  with  the  inertial  coordinate  system  {i} 

(see  Figs.  4  and  5).  Obviously,  in  this  case  the  attitude 
{c}  of  the  camera  with  respect  to  {i}  can  be  found 
using  (26)  from  the  transformation  matrix 
where  can  be  obtained  from  the  known  positions 
of  the  three  RPs  in  {/},  using  the  same  manner. 

V.  APPLICATION  TO  SHIPBOARD  NAVIGATION 

Next  we  present  a  simulation  example  where  the 
PSP  algorithm  is  applied  to  the  determination  of  the 
range  of  the  aircraft  with  respect  to  the  ship. 

The  simulation  scenario  is  shown  in  Figs.  14-15. 
The  ship  is  moving  North  at  a  constant  speed  of 
10  m/s.  Its  motion  is  characterized  by  pitch  and 
heave  oscillations  with  a  period  of  12  s.  The  aircraft 
is  performing  a  left  turn  with  descent  from  the 
initial  point  (—1450,  —200,  470)  m  with  respect 
to  the  ship’s  initial  position  at  an  airspeed  of  53 
m/s.  The  camera’s  focal  length  is  /  =  0.1  m  and 
declination  angle  with  respect  to  aircraft  longitudinal 
axis  is  —6°.  The  errors  in  the  projection  of  each  RP 
onto  the  image  plane  of  the  camera  are  modeled  as 
independent  Gaussian  random  process  with  zero  mean 
and  standard  deviation  of  one  pixel. 


Fig.  16.  Illustration  of  realistic  case  for  intersection  of  elliptic 
cylinders  in  (7). 


Fig.  17.  z-components  of  normals  generated  by  each  solution. 

Fig.  14  shows  the  horizontal  projection  of  each 
of  the  three  RPs  on  the  ship  tracked  by  the  camera 
and  of  the  aircraft’s  motion.  Fig.  15  gives  the 
corresponding  3D  representation. 

Fig.  16  contains  elliptical  cylinders  with  the 
coefficients  computed  from  the  data  taken  at  the  10^^ 
second  of  the  simulation. 

Fig.  17  includes  the  time  histories  of  the 
z-components  of  the  normals  generated  by 
each  solution.  The  z-component  of  the  normal 
corresponding  to  the  correct  solution  is  close  to  —  1 
(in  camera  coordinate  frame).  This  figure  also  shows 
how  the  algorithm  switches  between  the  two  solutions. 

Fig.  18  shows  the  differences  between  true  and 
estimated  values  of  the  components  of  the  vectors 
Pj  =  {X;,y;,z,},  i  =  1,...,3  versus  relative  range  to  the 
ship.  Clearly  the  errors  are  decreasing  as  the  aircraft 
approaches  the  ship. 

VI.  FLIGHT  TEST  SETUP  AND  DATA 

Naval  Postgraduate  School  has  recently  completed 
the  development  of  a  rapid  flight  test  prototyping 
system  (RFTPS)  for  a  prototype  UAV  named  Frog 
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high-level  development  language;  algorithms  are 
seamlessly  moved  from  the  high  level  design  and 
simulation  environment  to  the  real  time  processor;  the 
RFTPS  utilizes  industry  standard  I/O  including  digital 
to  analog,  analog  to  digital,  serial,  and  pulsewidth 
modulation  capabilities;  the  RFTPS  is  portable,  easily 
fitting  into  a  van.  In  general,  testing  will  occur  at 
fields  away  from  the  immediate  vicinity  of  the  Naval 
Postgraduate  School;  the  UAV  can  be  flown  manually, 
autonomously,  or  using  a  combination  of  the  two.  For 
instance,  automatic  control  of  the  lateral  axis  can  be 
tested  while  the  elevator  and  throttle  are  controlled 
manually;  all  I/O  and  internal  algorithm  variables 
can  be  monitored,  collected,  and  analyzed  within  the 
RFTPS  environment. 

To  test  the  developed  navigation  algorithms  the 
Frog  UAV  was  equipped  with  an  Infrared  Components 
Corporation  MB  IRES  IMAGE  CLEAR™  Uncooled 
Microbolometer  Module-based  IR  camera.  The  camera 
included  a  Boeing  U3000A  uncooled  8-12  jim  sensor 
and  the  Microbolometer  Module  which  produced 
National  Television  Standards  Committee  (NTSC) 
video  signal  and  output  it  via  a  RS-232  interface. 

The  focal  length  of  the  camera  lens  as  installed  in 
the  Erog  UAV  was  25  mm  with  a  field  of  view  of 
40°  X  30°.  The  pixel  resolution  of  the  camera  video 
was  320  X  240. 


Fig.  18.  Errors/range  history. 


[27].  The  RETPS  consists  of  a  test  bed  UAV  equipped 
with  an  avionics  suite  necessary  for  autonomous 
flight,  and  a  ground  station  responsible  for  flight 
control  of  the  UAV  and  flight  data  collection,  as 
shown  in  Pig.  19.  A  functional  block  diagram  of  the 
RETPS  is  also  shown  in  Pig.  19. 

The  RETPS  provides  the  following  capabilities. 
Within  the  RETPS  environment,  one  can  synthesize, 
analyze,  and  simulate  guidance,  navigation,  control, 
and  mission  management  algorithms  using  a 
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Fig.  19.  RFTPS  at  Naval  Postgraduate  School. 
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Fig.  20.  Flight  test  setup:  charcoal  grills  at  Camp  Roberts. 


The  camera  was  shock  mounted  in  the  nose  of  the 
aircraft  (see  Fig.  19),  and  the  pointing  angle  was  fixed 
in  the  x-z  plane  of  the  aircraft  body  axes,  declined 
5°  from  the  longitudinal  axis  of  the  aircraft.  As  a 
result  of  the  fixed  mounting  in  the  aircraft,  the  aircraft 
heading  and  attitude  alone  determined  the  camera 
pointing  angle.  Because  the  focal  length  of  the  camera 
was  fixed,  the  camera’s  field  of  view  was  fixed. 

The  IR  camera  video  signal  was  recorded  in  the 
Frog  using  a  Sony  Digital  Video  Walkman,  model 
GV-D300.  This  digital  video  tape  recorder  (VTR) 
recorded  the  live  NTSC  format  video  signal  from  the 
IR  camera  in  the  Digital  Video  (DV)  Standard  format 
on  a  DV  mini  video  magnetic  cassette  using  a  helical 
scan.  In  addition  to  the  video  image,  the  VTR  also 
recorded  the  elapsed  recording  time  of  each  video 
frame.  Flight  tests  in  support  of  this  project  were 
conducted  at  an  airfield  at  Camp  Roberts,  CA.  Three 
charcoal  grills  were  used  to  simulate  the  hot  spots  on 
the  ship  (see  Fig.  20). 

To  determine  the  precision  of  the  PSP  algorithm 
the  Frog  UAV  was  equipped  with  a  Trimble  AgCPS 


Fig.  21.  2D  representation  of  DGPS -recorded  trajectories. 

132  Differential  Global  Positioning  System  (DGPS). 
The  AgGPS  132  system  consisted  of  a  12  C/A-code 
channel  receiver,  a  combined  GPS/DGPS  receiver, 
and  a  ruggedized  antenna.  The  receiver  included 
ground  beacon  and  satellite  DGPS  capability.  The 
receiver  produced  messages  that  included  aircraft 
latitude,  longitude,  antenna  height  (altitude),  GPS 
quality  indication,  number  of  satellites,  horizontal 
dilution  of  precision,  speed  over  ground,  and  magnetic 
variation.  These  messages  were  transmitted  in  ASCII 
format  via  38KBaud  spread  spectrum  radio  frequency 
data  modems  to  the  ground  station.  Samples  of  UAV 
trajectories  recorded  by  onboard  DGPS  are  shown  in 
Fig.  21.  The  data  obtained  by  DGPS  together  with  the 
WGS-84  coordinates  of  the  charcoal  grills  was  used  to 
evaluate  the  accuracy  of  the  P3P  solution  during  post 
flight  analysis. 

VII.  FLIGHT-TEST  DATA  ANALYSIS 

The  landing  sequence  was  digitized  using  a  frame 
grabber  at  a  rate  of  30  Hz  (see  Fig.  22)  and  a  simple 


j'ii  'O’-'  tCT!* 


(b) 


Fig.  23.  Comparisons  of  IR  images,  (a)  Of  a  ship,  (b)  Of  the  three  hot  spots  at  Camp  Roberts. 


image-processing  algorithm  was  developed  to  identify  to  be  nontrivial  due  to  the  presence  of  multiple  hot 
three  hot  spots  on  the  runway  and  implemented  in  spots  in  the  surrounding  area.  This  is  in  contrast  to 

real-time  on  a  Pentium-II  PC.  finding  hot  spots  on  a  ship,  where  they  are  clearly 

The  image  processing  problem,  i.e.,  that  of  finding  much  hotter  than  the  ocean  (compare  plots  on  Fig. 

the  hot  spots  in  the  image  on  the  runway,  turned  out  23). 
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Fig.  24.  Main  ideas  of  the  first  step  for  IR  image  processing. 


Fig.  25.  Main  ideas  of  the  second  step  for  IR  image  processing. 
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Vltp 

Vertical  Position  Estimates  in  Local  Tangent  Plane  (LTP) 


y^TP  (m) 

(b) 


Fig.  26.  Isometric  (a)  and  plane  projections  (b)  of  DGPS  (green  line)  and  evaluated  (blue  dots)  positions  of  aircraft  with  respect  to 

three  bot  spots  (red  dots)  in  local- tangential-plane  coordinates. 


As  a  result  an  image  processing  algorithm  was 
developed  to  find  and  track  the  hot  spots  observed 
by  the  IR  camera  onboard  the  UAV  Frog  [28] .  The 
algorithm  consisted  of  two  steps.  The  first  step 
included  finding  the  hot  spots  in  the  initial  image  and 
involved  a  search  over  the  complete  image  plane  (see 
Fig.  24).  The  cornerstone  of  the  first  step  included  1) 
computing  a  running  average  of  each  row  of  pixels, 

2)  subtracting  the  average  from  the  actual  value  of 
each  pixel,  and  3)  and  selecting  the  point  that  exceed 
10  sigma  from  the  average.  Once  the  hot  spots  were 
found  in  the  initial  image,  they  were  tracked  for 
the  remainder  of  the  approach  (see  Fig.  25).  The 
tracking  algorithm  involved  1)  computing  a  bounding 
box  around  the  hot  spots,  2)  using  thresholding  and 
Gaussian  weighting  to  identify  the  groups  of  hot 
spots  corresponding  to  each  image  of  RP  within  the 
bounding  box,  and  3)  using  inertial  data  to  predict  the 
approximate  location  and  size  of  the  bounding  box  in 
the  next  image. 


450  3S0  250  150  50 

Horizontai  range  to  the  closest  hot  spot  (m) 

Fig.  27.  Error  between  evaluated  aircraft  position  and  GPS 
position  versus  range  to  hot  spots. 
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Error  (m) 


This  data  plus  the  geometry  of  three  hot  spots  with 
respect  to  the  runway  were  used  further  by  the  PSP 
algorithm  to  determine  range  and  orientation  of  the 
Frog  with  respect  to  the  runway. 

Results  of  this  application  are  shown  in  Fig.  26 
together  with  the  trajectory  obtained  by  the  onboard 
DGPS.  Fig.  27  shows  the  errors  between  the  DGPS 
positions  and  the  trajectories  computed  using  PSP 
algorithm.  Clearly,  the  algorithm  performed  as 
expected  with  the  total  error  decreasing  as  a  function 
of  range. 

VIII.  CONCLUSIONS 

In  this  paper  we  have  developed  a  new  efficient 
solution  to  the  PSP  problem  and  applied  it  to  UAV 
navigation  relative  to  the  shipboard.  We  have  shown 
that  in  this  particular  application  the  problem  has  only 
two  admissible  solutions,  and  we  have  developed  a 
numerical  algorithm  that  determines  both.  A  simple 
test  was  proposed  to  select  the  correct  admissible 
solution.  This  numerical  algorithm  was  tested  in 
simulation  and  using  flight  test  data.  The  accuracy  of 
the  resulting  solution  was  evaluated  using  onboard 
DGPS  system.  The  algorithm  was  determined  to 
perform  well.  Finally,  the  algorithm  was  implemented 
on  a  Pentium  II  computer  and  was  successfully  tested 
in  real-time  using  the  flight  test  data  provided  by  the 
onboard  IR  camera  at  20  Hz. 

REFERENCES 

[1]  Kaminer,  I.,  Kang,  W.,  Yakimenko,  O.,  and  Pascoal,  A. 

(2001) 

Application  of  nonlinear  filtering  to  navigation  system 
design  using  passive  sensors. 

IEEE  Transactions  on  Aerospace  and  Electronic  Systems, 
37,  1  (2001),  158-172. 

[2]  Hespanha,  J.,  Yakimenko,  O.,  Kaminer,  L,  and  Pascoal,  A. 

(2002) 

Linear  parametrically  varying  systems  with  brief 
instabilities:  An  application  to  integrated  vision/lMU 
navigation. 

In  Proceedings  of  the  40th  IEEE  Conference  on  Decision 
and  Control,  Orlando,  FL,  Dec.  12-15,  2002. 

[3]  Koshmieder,  H.  (1924) 

Theorie  der  Horizontalen  Sichtweite. 

Beitrage  zur  Physik  der  freien  Atmosphdre,  12  (1924), 
33-55, 171-181. 

[4]  Middleton,  W.  E.  K.  (1963) 

Vision  Through  the  Atmosphere. 

Toronto,  Canada:  Toronto  Press,  1963. 

[5]  Cooper,  A.  W.,  Lentz,  W.  J.,  Walker,  P.  L.,  and  Chan,  P.  M. 

(1994) 

Infrared  polarization  measurements  of  ship  signatures  and 
background  contrast. 

Proceedings  of  SPIE,  2223  (Jan.  1994),  300-310. 

[6]  Fischler,  M.  A.,  and  Bolles,  R.  C.  (1981) 

Random  sample  consensus:  A  paradigm  for  model 
fitting  with  applications  to  image  analysis  and  automated 
cartography. 

Communications  of  the  ACM,  24,  6  (1981),  381-395. 


[7]  Faugeras,  O.  D.,  and  Lustman,  F.  (1998) 

Motion  and  structure  from  motion  in  a  piecewise  planar 
environment. 

International  Journal  of  Pattern  Recognition  and  Artificial 
Intelligence,  2,  3  (1998),  485-508. 

[8]  Quan,  L.,  and  Lan,  Z.  (1999) 

Linear  N-point  camera  pose  determination. 

IEEE  Transactions  on  Pattern  Analysis  and  Machine 
Intelligence,  21,  8  (1999),  774-780. 

[9]  Church,  E.  (1948) 

Theory  of  photogrammetry. 

Bulletin  19:  Syracuse  University  Press,  Syracuse,  NY, 
1948. 

[10]  Grunert,  J.  A.  (1841) 

Das  Pothenotische  Problem  in  erweiterter  Gestalt  nebst 
Bilder  ueber  seine  Anwendungen  in  der  Geodasie. 
Grunerts  Archiv  fiir  Mathematik  und  Physik,  Band  1 
(1841),  238-248. 

[11]  Haralick,  R.  M.,  Lee,  C.,  Ottenberg,  K,.  and  Nolle,  M. 
(1991) 

Analysis  and  solutions  of  the  three  point  perspective  pose 
estimation  problem. 

In  Proceedings  of  the  IEEE  Conference  on  Computer 
Vision  and  Pattern  Recognition,  1991,  592-598. 

[12]  Muller,  F.  J.  (1925) 

Direkte  (exakte)  Ldsung  des  einfachen 
Riickw^tseinschneidens  im  Raume. 

Allgemeine  Vermessungs-Nachrichten,  1925. 

[13]  Sokolnikoff,  L,  and  Sokolnikoff,  E.  (1941) 

Higher  Mathematics  for  Engineers  and  Physicists. 

New  York:  McGraw-Hill,  1941. 

[14]  Merritt,  E.  L.  (1949) 

Explicity  three-point  resection  in  space. 

Photogrammetric  Engineering,  XV,  4  (1949),  649-655. 

[15]  Merritt,  E.  L.  (1949) 

General  explicit  equations  for  a  single  photograph. 
Analytical  Photogrammetry,  New  York:  Pitman,  1949, 
43-79. 

[16]  Finsterwalder,  S.,  and  Scheufele,  W.  (1937) 

Das  Riickw^tseinschneiden  im  Raum. 

Sebastian  Finstenvalder  zum  75-Geburtstage,  Verlag 
Herbert  Wichmann,  Berlin,  Germany,  1937,  86-100. 

[17]  Grafarend,  E.  W.,  Lohse,  P,  andSchaffrin,  B.  (1989) 

Dreidimensionaler  Riickwartsschnitt.  Teil  I:  Die 
projektiven  Gleichungen. 

Zeitschrift  fiir  Vermessungswesen,  Geodafcisches  Institut, 
Universitat  Stuttgart,  1989,  1-37. 

[18]  Lohse,  P.  (1989) 

Dreidimensionaler  Riickwartsschnitt  Ein  Algorithmus  zur 
Streckenberechnung  ohne  Hauptachsentransformation. 
Geodatisches  Institut,  Universitat  Stuttgart,  1989. 

[19]  Linnainmaa,  S.,  Hanvood,  D.,  and  Davis,  L.  S.  (1988) 

Pose  estimation  of  a  three-dimensional  object  using 
triangle  pairs. 

IEEE  Transactions  on  Pattern  Analysis  and  Machine 
Intelligence,  10,  5  (1988),  634-647. 

[20]  Slama,  C.  C.,  Theurer,  C.,  Henriksen,  S.  .  (Eds.)  (1979) 

Manual  of  Photogrammetry  (4th  ed.). 

Falls  Church,  VA:  American  Society  of  Photogrammetry, 
1980. 

[21]  Ghosh,  S.  K.  (1979) 

Analytical  Photogrammetry. 

New  York:  Pergamon  Press,  1979. 

[22]  Church,  E.  (1934) 

The  geometry  of  aerial  photograph. 

Syracuse,  NY:  Syracuse  University  Press,  1934. 


YAKIMENKO  ET  AL.:  UNMANNED  AIRCRAFT  NAVIGATION  FOR  SHIPBOARD  LANDING 


1199 


Authorized  licensed  use  limited  to:  Naval  Postgraduate  School.  Downloaded  on  March  1 1 ,2010  at  15:34:47  EST  from  IEEE  Xplore.  Restrictions  apply. 


[23]  Church,  E.  (1945) 

Revised  geometry  of  the  aerial  photograph. 

Syracuse,  NY:  Syracuse  University  Press,  Bulletin  15, 
1945. 

[24]  Moffitt,  F.  H.  (1959) 

Photogrammetry. 

Scranton,  PA:  International  Textbook  Co.,  1959. 

[25]  Weng,  J.,  Huang,  T.  S.,  and  Ahuja,  N.  (1993) 

Motion  and  Structure  from  image  sequences. 

New  York:  Springer  Verlag,  1993. 


[26]  Strang,  G.  (1976) 

Linear  Algebra  and  its  Applications. 

New  York:  Academic  Press,  1976. 

[27]  Hallberg,  E.,  Kaminer,  I.,  and  Pascoal,  A.  (1999) 

Development  of  a  flight  test  system  for  UAVs. 

IEEE  Control  Systems,  (Feb.  1999),  55-65. 

[28]  Ghyzel,  P.  (2000) 

Vision-based  navigation  for  autonomous  landing  of 
unmanned  aerial  vehicles. 

M.Sc.  Thesis,  Naval  Postgraduate  School,  Monterey,  CA, 
Sept.  2000. 


Oleg  Yakimenko  received  his  B.Sc.  and  M.Sc.  degrees  in  computer  science 
and  control  engineering  from  the  Moscow  Institute  of  Physics  and  Technology, 
Moscow,  Russia  in  1984  and  1986,  respectively.  In  1988  he  received  a  second 
M.Sc.  degree  in  aeronautical  engineering  and  operations  research  from  the  Air 
Force  Engineering  Academy  named  after  Professor  Nikolay  Zhukovskiy,  Moscow, 
Russia  (AFEA).  In  the  same  academy  he  received  the  degree  of  the  Candidate 
of  Technical  Sciences  (Ph.D.)  (1991)  and  Doctor  of  Technical  Sciences  (1996) 
specializing  in  optimal  control  theory  and  aeronautical  engineering. 

He  progressed  through  the  professorial  ranks  at  the  AFEA  and  since  late  1998 
has  been  a  Visiting  Professor  at  the  Naval  Postgraduate  School,  Monterey,  CA. 

His  research  interests  include  atmospheric  flight  mechanics,  optimal  control, 
integrated  guidance,  navigation  and  control  with  applications  to  UAV s  and 
parachutes,  and  human  factors. 

Dr.  Yakimenko  has  written  numerous  papers  in  the  areas  of  his  interests  and 
several  textbooks  for  graduate  courses  he  taught  at  the  AFEA.  He  is  an  Associate 
Fellow  of  the  Russian  Aviation  and  Aeronautics  Academy  of  Sciences  and  AIAA. 


Isaac  Kaminer  obtained  the  M.S.E.  degree  from  the  University  of  Minnesota, 
Minneapolis,  in  1985.  He  received  the  Ph.D.  degree  from  the  University  of 
Michigan,  Ann  Arbor,  in  1992. 

He  worked  for  the  Boeing  Company  between  his  M.S.E.  degree  and  Ph.D. 
degree,  first  on  the  757/767  program  and  then  in  the  guidance  and  control 
research  group.  He  is  currently  an  Associate  Professor  at  the  Department  of 
Aeronautics  and  Astronautics  at  the  Naval  Postgraduate  School,  Monterey, 

CA,  where  he  has  been  a  faculty  member  since  August  of  1992.  His  research 
interests  include  integrated  plant-controller  optimization  and  integrated  guidance, 
navigation  and  control  of  UAVs. 


Jerry  Lentz  received  his  B.Sc.  in  physics  in  1967  from  the  University  of 
North  Carolina,  Raleigh,  and  his  M.Sc.  in  nuclear  physics  in  1970  from  Purdue 
University,  Lafayette,  IN.  He  also  did  graduate  meteorology  study  at  the 
University  of  Arizona,  Tucson,  in  1977. 

He  joined  the  Naval  Postgraduate  School,  Monterey,  CA  in  1985  and 
is  currently  a  physicist  engineer  in  the  Department  of  Aeronautics  and 
Astronautics.  His  areas  of  publication  include  LIDAR,  mie  scattering,  continued 
fractions,  sonoluminescence,  high  speed  photon  counting,  infrared  image 
processing,  electronic  instrumentation,  particulate  sizing  and  analysis,  and  UAV 
in  strumentation . 


1200  IEEE  TRANSACTIONS  ON  AEROSPACE  AND  ELECTRONIC  SYSTEMS  VOL.  38,  NO.  4  OCTOBER  2002 


Authorized  licensed  use  limited  to:  Naval  Postgraduate  School.  Downloaded  on  March  1 1 ,2010  at  15:34:47  EST  from  IEEE  Xplore.  Restrictions  apply. 


